/*
 * Tracker.cpp
 *
 *  Created on: Nov 28, 2011
 *      Author: Ralph Schurade
 */
#include <fstream>
#include <iostream>
#include <algorithm>

#include <boost/lexical_cast.hpp>

/*** newmat include ***/
#define WANT_MATH
#define WANT_STREAM
#define use_namespace
#include "newmat/newmat.h"
#include "newmat/newmatap.h"
#include "newmat/newmatio.h"
#include "newmat/newmatrm.h"
#include "newmat/precisio.h"
using namespace NEWMAT;

#include "Tracker.h"

Tracker::Tracker( size_t minLength, float minFA, float stepSize, float smoothness ) :
	numGradients( 0 ),
	vDim( 0 ),
	numB0( 0 ),
	columns( 0 ),
	rows( 0 ),
	frames( 0 ),
	xVoxelSize( 1.0 ),
	yVoxelSize( 1.0 ),
	zVoxelSize( 1.0 ),

	m_minLength( minLength ),
	m_minFA( minFA ),
	m_stepSize( stepSize ),
	maxStepsInVoxel( 3 ),
	m_smoothness( smoothness )
{
	// TODO Auto-generated constructor stub

}

Tracker::~Tracker()
{
	// TODO Auto-generated destructor stub
}

bool Tracker::load( vector<string> filenames )
{
	vector<float> inputBvecs;
	vector<int> inputBvals;

	header = nifti_image_read( filenames[0].c_str(), 0 );

	// if a proper nifti header is found try to load the gradient and bval files
	if ( header )
	{
		vDim = header->dim[4];

		cout << "nifti file contains " << vDim << " images" << endl;

					// check if the file exists
		ifstream i( filenames[1].c_str() );
		if( i.bad() || !i.is_open() )
		{
			if( i.is_open() )
			{
				i.close();
			}
		}
		else
		{
			// read gradients

			// the file should contain the x-coordinates in line 0, y-coordinates in line 1,
			// z-coordinates in line 2
			inputBvecs.resize( 3 * vDim );
			for( unsigned int j = 0; j < 3; ++j )
			{
				for( size_t k = 0; k < vDim; ++k )
				{
					string val;
					i >> val;
					inputBvecs[k*3 + j] = boost::lexical_cast<float>( val );
				}
			}
			//bool success = !i.eof();
			i.close();
		}

		// check if the file exists
		ifstream ii( filenames[2].c_str() );
		if( ii.bad() || !ii.is_open() )
		{
			if( ii.is_open() )
			{
				ii.close();
			}
		}
		else
		{
			inputBvals.resize( vDim );
			for( size_t k = 0; k < vDim; ++k )
			{
				string val;
				ii >> val;
				inputBvals[k] = boost::lexical_cast<int>( val );
				if ( inputBvals[k] != 0 )
				{
					bvals.push_back( inputBvals[k] );
					++numGradients;
				}
			}

			//bool success = !ii.eof();
			ii.close();
		}
	}
	else // if ( header )
	{
		cout << "failed to find nifti header" << endl;
		return false;
	}

	cout << numGradients << " non zero bvals" << endl;

	numB0 = vDim - numGradients;

	columns = header->dim[1];
	rows = header->dim[2];
	frames = header->dim[3];
	blockSize = rows* columns * frames;

	xVoxelSize = header->dx;
	yVoxelSize = header->dy;
	zVoxelSize = header->dz;

	dti.resize( numGradients * blockSize );
	b0Images.resize( blockSize, 0.0 );

	nifti_image* filedata = nifti_image_read( filenames[0].c_str(), 1 );
	int16_t *inputData;

	if ( header->datatype == 4 )
	{
		inputData = reinterpret_cast< int16_t* >( filedata->data );
	}
	else
	{
		cout << "datatype not yet supported" << endl;
		return false;
	}

	if ( m_mask.size() == 0 )
	{
		m_mask.resize( blockSize, 1 );
	}

	size_t curGrad = 0;

	for ( size_t i = 0; i < vDim; ++i )
	{
		if ( inputBvals[i] != 0 )
		{
			// copy to dti
			for ( size_t j = 0; j < blockSize; ++j )
			{
				if ( m_mask[j] > 0 )
					dti[curGrad*blockSize + j] = inputData[i* blockSize + j];
			}
			curGrad++;
		}
		else
		{
			//cout << "copy image #" << i << " to b0images" << endl;
			// copy to b0 images
			for ( size_t j = 0; j < blockSize; ++j )
			{
				if ( m_mask[j] > 0 )
					b0Images[j] += (float)inputData[i* blockSize + j] / (float)numB0;
			}
		}
	}
/*
	vector<float>dtifloat(blockSize*60);
	for(size_t i = 0; i < blockSize*60; ++i)
	{
		dtifloat[i] = (float)dti[i];
	}
*/
	writeNifti( "b0.nii.gz", b0Images, 1 );
	//writeNifti( "dti.nii.gz", dtifloat, 60 );

	for ( size_t i = 0; i < vDim; ++i )
	{
		if ( inputBvals[i] != 0 )
		{
			bvecs.push_back( inputBvecs[i*3] );
			bvecs.push_back( inputBvecs[i*3+1] );
			bvecs.push_back( inputBvecs[i*3+2] );
		}
	}

	delete filedata;

	return true;
}

bool Tracker::loadMask( string fn )
{
	maskHeader = nifti_image_read( fn.c_str(), 0 );
	if ( !maskHeader )
	{
		return false;
	}
	int cls = maskHeader->dim[1];
	int rws = maskHeader->dim[2];
	int fms = maskHeader->dim[3];
	size_t bs = rws* cls * fms;

	nifti_image* filedata = nifti_image_read( fn.c_str(), 1 );
	unsigned char *inputData;

	if ( maskHeader->datatype == 2 )
	{
		inputData = reinterpret_cast< unsigned char* >( filedata->data );
	}
	m_mask.resize( bs );
	for ( size_t i = 0; i < bs; ++i )
	{
		m_mask[i] = inputData[i];
	}
	delete filedata;
	delete inputData;
	return true;
}

void Tracker::saveTensors()
{
	vector<float>data( blockSize * 6 );
	for( size_t i = 0; i < blockSize; ++i )
	{
		data[i] = tensors[i*6];
		data[i + blockSize] = tensors[i*6+1];
		data[i + blockSize*2] = tensors[i*6+2];
		data[i + blockSize*3] = tensors[i*6+3];
		data[i + blockSize*4] = tensors[i*6+4];
		data[i + blockSize*5] = tensors[i*6+5];
	}

	writeNifti( "tensors.nii.gz", data, 6 );
}

void Tracker::saveFA()
{
	writeNifti( "fa.nii.gz", fa, 1 );
}

void Tracker::saveEvec()
{
	vector<float>data( blockSize * 3 );
	for( size_t i = 0; i < blockSize; ++i )
	{
		data[i] = evec1[i*3];
		data[i + blockSize] = evec1[i*3+1];
		data[i + blockSize*2] = evec1[i*3+2];
	}
	writeNifti( "evec.nii.gz", data, 3 );
}


void Tracker::writeNifti( string fn, vector<float>data, int dim )
{
	nifti_image *outField = nifti_simple_init_nim();

	//size_t nbValues = blockSize;

	outField->nx = columns;
	outField->ny = rows;
	outField->nz = frames;

	outField->nvox = blockSize * dim;

	outField->dx = header->dx;
	outField->dy = header->dy;
	outField->dz = header->dz;

	outField->nifti_type = 1; // 1==NIFTI-1 (1 file)

	outField->freq_dim = 1;
	outField->phase_dim = 2;
	outField->slice_dim = 3;

	outField->qform_code = 1;
	outField->sform_code = 1;

	outField->nt = dim;
	outField->nv = 4;

	outField->ndim = 4;

	//std::string description = "tensors";
	// a description max. 80 char
	//description.copy( outField->descrip, 80 );

	for( size_t i = 0; i < 4; ++i )
	{
		for( size_t j = 0; j < 4; ++j )
		{
			outField->qto_xyz.m[i][j] = header->qto_xyz.m[i][j];
			outField->sto_xyz.m[i][j] = header->sto_xyz.m[i][j];
		}
	}

	{
		float dx, dy, dz;
		nifti_mat44_to_quatern( outField->qto_xyz, &( outField->quatern_b ),
								&( outField->quatern_c ), &( outField->quatern_d ),
								&( outField->qoffset_x ), &( outField->qoffset_y ),
								&( outField->qoffset_z ),
								&dx, &dy, &dz,
								&( outField->qfac ) );
	}

	outField->qto_ijk = nifti_mat44_inverse( outField->qto_xyz );
	outField->sto_ijk = nifti_mat44_inverse( outField->sto_xyz );

	outField->nbyper = 4;
	outField->datatype = DT_FLOAT;

	outField->data = &(data[0]);
	if( nifti_set_filenames( outField, fn.c_str(), 0, 1 ) )
	{
		cout << "NIfTI filename Problem" << endl;
	}

	nifti_image_write( outField );

	outField->data = NULL;
	nifti_image_free( outField );
}

void Tracker::saveFibs()
{
	using std::fstream;
	fstream out( "fibers.fib", fstream::out | fstream::in | fstream::trunc );
	if( !out || out.bad() )
	{
		// error
	}

	vector<float>m(9);
	for( size_t i = 0; i < 3; ++i )
	{
		for( size_t j = 0; j < 3; ++j )
		{
			m[i*3+j] = header->qto_xyz.m[i][j] / header->dx;
		}
	}

	// We use '\n' as line delimiter so also files written under windows (having '\r\n' as delimtier) may be read anywhere
	char lineDelimiter = '\n';

	out << "# vtk DataFile Version 3.0" << lineDelimiter;
	out << "Fibers" << lineDelimiter;
	out << "BINARY" << lineDelimiter;
	out << "DATASET POLYDATA" << lineDelimiter;

	unsigned int numPoints = 0;
	unsigned int numLines = fibs.size();
	for( size_t i = 0; i < fibs.size(); ++i )
	{
		numPoints += fibs[i].size()/3;
	}
	out << "POINTS " << numPoints << " float" << lineDelimiter;
	unsigned int *rawLineData = new unsigned int[numPoints + numLines];
	float *rawPointData = new float[numPoints * 3];

	unsigned int pntPosOffset = 0;
	unsigned int lnsPosOffset = 0;
	for( size_t i = 0; i < fibs.size(); ++i )
	{
		vector<float>fib = fibs[i];
		rawLineData[lnsPosOffset++] = static_cast< unsigned int >( fib.size()/3 );
		for( size_t j = 0; j < fib.size()/3; ++j )
		{
			rawLineData[lnsPosOffset++] = static_cast< unsigned int >( pntPosOffset / 3 );
			/******************************************************************************
			 * transform vertex coordinates
			 */
			fib[j*3]   = m[0] * fib[j*3] + m[1] * fib[j*3+1] + m[2] * fib[j*3+2];
			fib[j*3+1] = m[3] * fib[j*3] + m[4] * fib[j*3+1] + m[5] * fib[j*3+2];
			fib[j*3+2] = m[6] * fib[j*3] + m[7] * fib[j*3+1] + m[8] * fib[j*3+2];
			fib[j*3] += header->qto_xyz.m[0][3];
			fib[j*3+1] += header->qto_xyz.m[1][3];
			fib[j*3+2] += header->qto_xyz.m[2][3];

			rawPointData[pntPosOffset++] = static_cast< float >( fib[j*3] );
			rawPointData[pntPosOffset++] = static_cast< float >( fib[j*3+1] );
			rawPointData[pntPosOffset++] = static_cast< float >( fib[j*3+2] );
		}
	}
	switchByteOrderOfArray< float >( rawPointData, numPoints * 3 );
	switchByteOrderOfArray< unsigned int >( rawLineData, numLines + numPoints );
	out.write( reinterpret_cast< char* >( rawPointData ), sizeof( float ) * numPoints * 3 );
	out << lineDelimiter;
	out << "LINES " << numLines << " " << numPoints + numLines << lineDelimiter;
	out.write( reinterpret_cast< char* >( rawLineData ), sizeof( unsigned int ) * ( numPoints + numLines ) );
	out << lineDelimiter;
	out.close();
}

void Tracker::fitTensors()
{
	int N = numGradients;

	Matrix B(N,6);
	Matrix U(N,6);
	Matrix V(6,6);
	Matrix BI(6,N);
	DiagonalMatrix D(6);

	double mult_c;

	for ( int i=0; i < N; ++i )
	{
		mult_c = (float)bvals[i] / (float)( bvecs[i*3]*bvecs[i*3] + bvecs[i*3+1]*bvecs[i*3+1] + bvecs[i*3+2]*bvecs[i*3+2] );

		B(i+1,1) =   mult_c * bvecs[i*3]  *bvecs[i*3];
		B(i+1,2) = 2*mult_c * bvecs[i*3]  *bvecs[i*3+1];
		B(i+1,3) = 2*mult_c * bvecs[i*3]  *bvecs[i*3+2];
		B(i+1,4) =   mult_c * bvecs[i*3+1]*bvecs[i*3+1];
		B(i+1,5) = 2*mult_c * bvecs[i*3+1]*bvecs[i*3+2];
		B(i+1,6) =   mult_c * bvecs[i*3+2]*bvecs[i*3+2];
	}

	SVD(B, D, U, V);

	for( int j=1; j<=6;++j )
	{
		D(j)=1./D(j);
	}
	BI=V*D*U.t();

	double s0 = 0.0;
	double si = 0.0;
	vector<double>log_s0_si_pixel(numGradients);
	tensors.resize( blockSize*6 );

	for ( size_t i = 0; i < blockSize; ++i )
	{
		s0 = b0Images[i];

		if ( s0 > 0 )
		{
			/* compute log(s0)-log(si) */
			s0 = log(s0);
			for ( size_t j=0; j<numGradients; ++j )
			{
				si = dti[j*blockSize+i];
				if (si > 0)
				{
					si = log(si);
				}
				else
				{
					si = 0.0;
				}
				log_s0_si_pixel[j] = s0 - si;
			}

			double value;
			/* compute tensor*/
			for( int l=0; l<6; l++ )
			{
				value=0;
				for( size_t m=1; m<=numGradients; ++m )
				{
					value += BI(l+1,m) * log_s0_si_pixel[m-1];
				}
				//tensors[blockSize*l+i] = (float) (value); //save each component in a frame
				tensors[i*6+l] = (float) (value); // save the tensor components in a adjacent memory
			}

		} // end if s0 > 0
	}
}

void Tracker::createLogTensors()
{
	cout << "create log tensors" << endl;
	logTensors.resize( tensors.size() );
	//log(M) =Ulog(D)UT
	Matrix U(3,3);
	DiagonalMatrix D(3);
	Matrix logM(3,3);
	for ( size_t i = 0; i < blockSize; ++i )
	{
		U( 1, 1 ) = evec1[3*i];
		U( 2, 1 ) = evec1[3*i+1];
		U( 3, 1 ) = evec1[3*i+2];
		U( 1, 2 ) = evec2[3*i];
		U( 2, 2 ) = evec2[3*i+1];
		U( 3, 2 ) = evec2[3*i+2];
		U( 1, 3 ) = evec3[3*i];
		U( 2, 3 ) = evec3[3*i+1];
		U( 3, 3 ) = evec3[3*i+2];
		D(1) = log( eval1[i] );
		D(2) = log( eval2[i] );
		D(3) = log( eval3[i] );

		logM = U*D*U.t();

		logTensors[i*6] = logM(1,1);
		logTensors[i*6+1] = logM(1,2);
		logTensors[i*6+2] = logM(1,3);
		logTensors[i*6+3] = logM(2,2);
		logTensors[i*6+4] = logM(2,3);
		logTensors[i*6+5] = logM(3,3);
	}
}

void Tracker::computeTrace()
{
	cout << "compute trace" << endl;
	trace.resize( blockSize );
	float value = 0;
	for ( size_t i = 0; i < blockSize; ++i )
	{
		value = tensors[i*6];
		value += tensors[i * 6 + 3];
		value += tensors[i * 6 + 5];
		trace[i] = value/3.0;
	}
}

void Tracker::computeFA()
{
	cout << "compute fa" << endl;
	fa.resize( blockSize );
	double xx,xy,xz,yy,yz,zz,tr,AA,DD;
	for ( size_t i = 0; i < blockSize; ++i )
	{
		xx = tensors[i * 6];
		xy = tensors[i * 6 + 1];
		xz = tensors[i * 6 + 2];
		yy = tensors[i * 6 + 3];
		yz = tensors[i * 6 + 4];
		zz = tensors[i * 6 + 5];
		tr = trace[i];

		AA = pow2(xx-tr)+ pow2(yy-tr)+ pow2(zz-tr)+ 2*pow2(xy)+ 2*pow2(xz)+ 2*pow2(yz);
		DD = pow2(xx)+ pow2(yy)+ pow2(zz)+ 2*pow2(xy)+ 2*pow2(xz)+ 2*pow2(yz);

		if (DD>0)
		{
			fa[i] = (float) ( sqrt( AA ) / sqrt( DD ) * sqrt( 1.5 ) );
		}
		else
		{
			fa[i] = 0.0;
		}
	}
}

void Tracker::computeEigen()
{
	cout << "compute eigen" << endl;
	double xx,xy,xz,yy,yz,zz;
	double i1, i2, i3, v, s, phi, l1, l2, l3;
	double ev1_x, ev1_y, ev1_z, ev2_x, ev2_y, ev2_z, ev3_x, ev3_y, ev3_z, vec_norm;
	maxEval = 0.0;

	eval1.resize( blockSize );
	eval2.resize( blockSize );
	eval3.resize( blockSize );
	evec1.resize( blockSize * 3);
	evec2.resize( blockSize * 3);
	evec3.resize( blockSize * 3);

	for ( size_t i = 0; i < blockSize; ++i )
	{
		xx = tensors[i * 6];
		xy = tensors[i * 6 + 1];
		xz = tensors[i * 6 + 2];
		yy = tensors[i * 6 + 3];
		yz = tensors[i * 6 + 4];
		zz = tensors[i * 6 + 5];

		// three invariants of D (dt)
		// i1=l1+l2+l3 (trace)
		// i2=l1*l2+l1*l3+l2*l3
		// i3=l1*l2*l3 (determinante)
		i1 = xx + yy + zz;
		i2 = xx*yy + xx*zz + yy*zz - ( pow2(xy) + pow2(xz) + pow2(yz) );
		i3 = xx*yy*zz+ 2.*xy*xz*yz - ( zz*pow2(xy) + yy*pow2(xz) + xx*pow2(yz));

		v = pow2(i1/3)-i2/3;
		s = pow3(i1/3)-i1*i2/6+i3/2;
		if ((v > 0) && (pow2(s) < pow3(v)))
		  phi=acos(s/v*sqrt(1./v))/3;
		else phi = 0;

		// eigenvalues
		if (phi !=0) {
		  l1=i1/3+2*sqrt(v)*cos(phi);
		  l2=i1/3-2*sqrt(v)*cos(M_PI/3.+phi);
		  l3=i1/3-2*sqrt(v)*cos(M_PI/3.-phi);
		}
		else
		  l1=l2=l3=0.0;

		maxEval = max( max( max( maxEval, l1 ), l2 ), l3 );
		eval1[i] = l1;
		eval2[i] = l2;
		eval3[i] = l3;

		// eigenvectors
		ev1_x = (xy*yz-(yy-l1)*xz)* (xz*yz-(zz-l1)*xy);
		ev1_y = (xz*yz-(zz-l1)*xy)* (xz*xy-(xx-l1)*yz);
		ev1_z = (xy*yz-(yy-l1)*xz)* (xz*xy-(xx-l1)*yz);

		ev2_x = (xy*yz-(yy-l2)*xz)* (xz*yz-(zz-l2)*xy);
		ev2_y = (xz*yz-(zz-l2)*xy)* (xz*xy-(xx-l2)*yz);
		ev2_z = (xy*yz-(yy-l2)*xz)* (xz*xy-(xx-l2)*yz);

		ev3_x = (xy*yz-(yy-l3)*xz)* (xz*yz-(zz-l3)*xy);
		ev3_y = (xz*yz-(zz-l3)*xy)* (xz*xy-(xx-l3)*yz);
		ev3_z = (xy*yz-(yy-l3)*xz)* (xz*xy-(xx-l3)*yz);

		vec_norm = sqrt(pow2(ev1_x)+pow2(ev1_y)+pow2(ev1_z));

		if (vec_norm >0) {
		  ev1_x = ev1_x/vec_norm;
		  ev1_y = ev1_y/vec_norm;
		  ev1_z = ev1_z/vec_norm;
		}
		else
		  ev1_x=ev1_y=ev1_z=0.0;

		vec_norm=sqrt(pow2(ev2_x)+pow2(ev2_y)+pow2(ev2_z));

		if (vec_norm >0) {
		  ev2_x = ev2_x/vec_norm;
		  ev2_y = ev2_y/vec_norm;
		  ev2_z = ev2_z/vec_norm;
		}
		else
		  ev2_x=ev2_y=ev2_z=0.0;

		vec_norm=sqrt(pow2(ev3_x)+pow2(ev3_y)+pow2(ev3_z));

		if (vec_norm >0) {
		  ev3_x = ev3_x/vec_norm;
		  ev3_y = ev3_y/vec_norm;
		  ev3_z = ev3_z/vec_norm;
		}
		else
		  ev3_x=ev3_y=ev3_z=0.0;

		evec1[i*3  ] = ev1_x;
		evec1[i*3+1] = ev1_y;
		evec1[i*3+2] = ev1_z;
		evec2[i*3  ] = ev2_x;
		evec2[i*3+1] = ev2_y;
		evec2[i*3+2] = ev2_z;
		evec3[i*3  ] = ev3_x;
		evec3[i*3+1] = ev3_y;
		evec3[i*3+2] = ev3_z;
	}
	evalScale = 2.0 / maxEval;
	createLogTensors();
}

void Tracker::startTracking()
{
	cout << "start tracking" << endl;
	vector<float> fib1;
	vector<float> fib2;
	vector<float> fib2r;
	int numPoints = 0;
	int numLines = 0;
	size_t j = 0;

	float diag = sqrt( header->dx * header->dx + header->dy * header->dy + header->dz * header->dz );
	maxStepsInVoxel = (int)( diag / m_stepSize) + 1;
	cout << "diag: " << diag << " msiv: " << maxStepsInVoxel << endl;
	cout << "smoothness: " << m_smoothness << endl;
	for( size_t z = 0; z < frames; ++z )
	{
		cout << "finished frame " << z << "/" << frames-1 << endl;
		for( size_t y = 0; y < rows; ++y )
		{
			for ( size_t x = 0; x < columns; ++x )
			{
				//if ( x%3 == 0 ) {
				fib1 = track( x, y, z, false );
				fib2 = track( x, y, z, true );
				fib2r.resize( fib2.size() );
				for( size_t i = 0; i < fib2.size() / 3; ++i )
				{
					j = i*3;
					fib2r[j] = fib2[(fib2.size()-1)-(j+2)];
					fib2r[j+1] = fib2[(fib2.size()-1)-(j+1)];
					fib2r[j+2] = fib2[(fib2.size()-1)-j];
				}

				fib2r.insert( fib2r.end(), fib1.begin(), fib1.end() );

				if ( fib2.size()/3 >= m_minLength )
				{
					fibs.push_back( fib2r );
					numPoints += fib2r.size()/3;
					numLines++;
				}
				//}
			}
		}
	}
	cout << numLines << " lines with "<< numPoints << " points." << endl;
}

vector<float> Tracker::track( size_t xs, size_t ys, size_t zs, bool negDir )
{
	vector<float> result;
	double xx,xy,xz,yy,yz,zz;
	float newDirX, newDirY, newDirZ;
	float dirX, dirY, dirZ, norm;
	size_t oldId;
	vector<float>iT; // interpolated tensor

	size_t id = xs + ys * columns + zs * rows * columns;
	float x = ( xs + 0.5 ) * xVoxelSize;
	float y = ( ys + 0.5 ) * yVoxelSize;
	float z = ( zs + 0.5 ) * zVoxelSize;

	if ( negDir )
	{
		dirX = evec1[3*id  ] * -1.0;
		dirY = evec1[3*id+1] * -1.0;
		dirZ = evec1[3*id+2] * -1.0;
	}
	else
	{
		float norm = sqrt( evec1[3*id]*evec1[3*id] + evec1[3*id+1]*evec1[3*id+1] + evec1[3*id+2]*evec1[3*id+2] );
		dirX = evec1[3*id  ]/norm* m_stepSize;
		dirY = evec1[3*id+1]/norm* m_stepSize;
		dirZ = evec1[3*id+2]/norm* m_stepSize;
	}

	bool breakCondition = false;
	int lc = 0;
	while ( !breakCondition )
	{
		if ( m_mask[id] > 0 && fa[id] > m_minFA )
		{
			result.push_back( x );
			result.push_back( y );
			result.push_back( z );
		}
		else
		{
			breakCondition = true;
		}

		x += dirX;
		y += dirY;
		z += dirZ;

		id = getID( x, y, z );

		if ( oldId == id )
		{
			++lc;
			if ( lc > maxStepsInVoxel )
			{
				breakCondition = true;
			}
		}
		else
			lc = 0;

		iT = getInterpolatedTensor( id, x, y, z );

		xx = iT[0];
		xy = iT[1];
		xz = iT[2];
		yy = iT[3];
		yz = iT[4];
		zz = iT[5];

		// dir = tensor(xyz) * dir;
		newDirX = xx * dirX + xy * dirY + xz * dirZ;
		newDirY = xy * dirX + yy * dirY + yz * dirZ;
		newDirZ = xz * dirX + yz * dirY + zz * dirZ;

		norm = sqrt( newDirX * newDirX + newDirY * newDirY + newDirZ * newDirZ);
		newDirX = newDirX / norm;
		newDirY = newDirY / norm;
		newDirZ = newDirZ / norm;

//		newDirX = m_smoothness * dirX + ( 1.0 - m_smoothness ) * newDirX;
//		newDirY = m_smoothness * dirY + ( 1.0 - m_smoothness ) * newDirY;
//		newDirZ = m_smoothness * dirZ + ( 1.0 - m_smoothness ) * newDirZ;


		if ( ( dirX * iT[7] + dirY * iT[8] + dirZ * iT[9] ) < 0 )
		{
			newDirX = iT[6] * iT[7] * -1.0 + ( 1.0 - iT[6] ) * ( m_smoothness * dirX + ( 1.0 - m_smoothness ) * newDirX );
			newDirY = iT[6] * iT[8] * -1.0 + ( 1.0 - iT[6] ) * ( m_smoothness * dirY + ( 1.0 - m_smoothness ) * newDirY );
			newDirZ = iT[6] * iT[9] * -1.0 + ( 1.0 - iT[6] ) * ( m_smoothness * dirZ + ( 1.0 - m_smoothness ) * newDirZ );
		}
		else
		{
			newDirX = iT[6] * iT[7] + ( 1.0 - iT[6] ) * ( m_smoothness * dirX + ( 1.0 - m_smoothness ) * newDirX );
			newDirY = iT[6] * iT[8] + ( 1.0 - iT[6] ) * ( m_smoothness * dirY + ( 1.0 - m_smoothness ) * newDirY );
			newDirZ = iT[6] * iT[9] + ( 1.0 - iT[6] ) * ( m_smoothness * dirZ + ( 1.0 - m_smoothness ) * newDirZ );
		}

		norm = sqrt( newDirX * newDirX + newDirY * newDirY + newDirZ * newDirZ);
		dirX = newDirX / norm * m_stepSize;
		dirY = newDirY / norm * m_stepSize;
		dirZ = newDirZ / norm * m_stepSize;
		oldId = id;
	}
	return result;
}

size_t Tracker::getID( float x, float y, float z )
{
	size_t id = (int)(x/xVoxelSize) + (int)(y/yVoxelSize) * columns + (int)(z/zVoxelSize) * rows * columns;

	id = max( (size_t)0, min( blockSize, id ) );
	return id;
}

vector<float> Tracker::getInterpolatedTensor( size_t id, float inx, float iny, float inz )
{
	float x = inx/xVoxelSize;
	float y = iny/yVoxelSize;
	float z = inz/zVoxelSize;

	int x0 = (int)x;
	int y0 = (int)y;
	int z0 = (int)z;

	//cout << "x: " << inx << " y: " << iny << " z: " << inz << endl;
	//cout << "x0: " << x0 << " y0: " << y0 << " z0: " << z0 << endl;

	float xd = x - x0;
	float yd = y - y0;
	float zd = z - z0;
	//cout << "xd: " << xd << " yd: " << yd << " zd: " << zd << endl;

	size_t id_x0y0z0 = id;
	size_t id_x1y0z0 = id + 1;
	size_t id_x0y1z0 = id + columns;
	size_t id_x1y1z0 = id + columns + 1;
	size_t id_x0y0z1 = id + columns * rows;
	size_t id_x1y0z1 = id + columns * rows + 1;
	size_t id_x0y1z1 = id + columns * rows + columns;
	size_t id_x1y1z1 = id + columns * rows + columns + 1;

	vector<float>i1(6);
	vector<float>i2(6);
	vector<float>j1(6);
	vector<float>j2(6);
	vector<float>w1(6);
	vector<float>w2(6);
	vector<float>iv(10);
	for ( size_t i = 0; i < 6; ++i )
	{
		i1[i] = logTensors[id_x0y0z0 * 6 + i] * ( 1.0 - zd ) + logTensors[id_x0y0z1 * 6 + i] * zd;
		i2[i] = logTensors[id_x0y1z0 * 6 + i] * ( 1.0 - zd ) + logTensors[id_x0y1z1 * 6 + i] * zd;
		j1[i] = logTensors[id_x1y0z0 * 6 + i] * ( 1.0 - zd ) + logTensors[id_x1y0z1 * 6 + i] * zd;
		j2[i] = logTensors[id_x1y1z0 * 6 + i] * ( 1.0 - zd ) + logTensors[id_x1y1z1 * 6 + i] * zd;
	}
	for ( size_t i = 0; i < 6; ++i )
	{
		w1[i] = i1[i] * ( 1.0 - yd ) + i2[i] * yd;
		w2[i] = j1[i] * ( 1.0 - yd ) + j2[i] * yd;
	}
	for ( size_t i = 0; i < 6; ++i )
	{
		iv[i] = w1[i] * ( 1.0 - xd ) + w2[i] * xd;
	}
	/*
	cout << "***********" << endl;
	cout << "iv: " << iv[0] << " " << iv[1] << " " << iv[2] << " " << iv[3] << " " << iv[4] << " " << iv[5] << " " << endl;
	cout << "xxx:  " << tensors[id_x0y0z0*6] << " " << tensors[id_x0y0z0*6+1] << " " << tensors[id_x0y0z0*6+2] << " " << tensors[id_x0y0z0*6+3] << " " << tensors[id_x0y0z0*6+4] << " " << tensors[id_x0y0z0*6+5] << " " << endl;
	expT(logT(id_x0y0z0));
	cout << "***********" << endl;
	*/
	return expT( iv );
}

vector<float>Tracker::expT( vector<float> t )
{
	double xx,xy,xz,yy,yz,zz;
	double i1, i2, i3, v, s, phi, l1, l2, l3;
	double ev1_x, ev1_y, ev1_z, ev2_x, ev2_y, ev2_z, ev3_x, ev3_y, ev3_z, vec_norm;

	xx = t[0];
	xy = t[1];
	xz = t[2];
	yy = t[3];
	yz = t[4];
	zz = t[5];

	// three invariants of D (dt)
	// i1=l1+l2+l3 (trace)
	// i2=l1*l2+l1*l3+l2*l3
	// i3=l1*l2*l3 (determinante)
	i1 = xx + yy + zz;
	i2 = xx*yy + xx*zz + yy*zz - ( pow2(xy) + pow2(xz) + pow2(yz) );
	i3 = xx*yy*zz+ 2.*xy*xz*yz - ( zz*pow2(xy) + yy*pow2(xz) + xx*pow2(yz));

	v = pow2(i1/3)-i2/3;
	s = pow3(i1/3)-i1*i2/6+i3/2;
	if ((v > 0) && (pow2(s) < pow3(v)))
	  phi=acos(s/v*sqrt(1./v))/3;
	else phi = 0;

	// eigenvalues
	if (phi !=0) {
	  l1=i1/3+2*sqrt(v)*cos(phi);
	  l2=i1/3-2*sqrt(v)*cos(M_PI/3.+phi);
	  l3=i1/3-2*sqrt(v)*cos(M_PI/3.-phi);
	}
	else
	  l1=l2=l3=0.0;
/*
	eval1[i] = l1;
	eval2[i] = l2;
	eval3[i] = l3;
*/
	// eigenvectors
	ev1_x = (xy*yz-(yy-l1)*xz)* (xz*yz-(zz-l1)*xy);
	ev1_y = (xz*yz-(zz-l1)*xy)* (xz*xy-(xx-l1)*yz);
	ev1_z = (xy*yz-(yy-l1)*xz)* (xz*xy-(xx-l1)*yz);

	ev2_x = (xy*yz-(yy-l2)*xz)* (xz*yz-(zz-l2)*xy);
	ev2_y = (xz*yz-(zz-l2)*xy)* (xz*xy-(xx-l2)*yz);
	ev2_z = (xy*yz-(yy-l2)*xz)* (xz*xy-(xx-l2)*yz);

	ev3_x = (xy*yz-(yy-l3)*xz)* (xz*yz-(zz-l3)*xy);
	ev3_y = (xz*yz-(zz-l3)*xy)* (xz*xy-(xx-l3)*yz);
	ev3_z = (xy*yz-(yy-l3)*xz)* (xz*xy-(xx-l3)*yz);

	vec_norm = sqrt(pow2(ev1_x)+pow2(ev1_y)+pow2(ev1_z));

	if (vec_norm >0) {
	  ev1_x = ev1_x/vec_norm;
	  ev1_y = ev1_y/vec_norm;
	  ev1_z = ev1_z/vec_norm;
	}
	else
	  ev1_x=ev1_y=ev1_z=0.0;

	vec_norm=sqrt(pow2(ev2_x)+pow2(ev2_y)+pow2(ev2_z));

	if (vec_norm >0) {
	  ev2_x = ev2_x/vec_norm;
	  ev2_y = ev2_y/vec_norm;
	  ev2_z = ev2_z/vec_norm;
	}
	else
	  ev2_x=ev2_y=ev2_z=0.0;

	vec_norm=sqrt(pow2(ev3_x)+pow2(ev3_y)+pow2(ev3_z));

	if (vec_norm >0) {
	  ev3_x = ev3_x/vec_norm;
	  ev3_y = ev3_y/vec_norm;
	  ev3_z = ev3_z/vec_norm;
	}
	else
	  ev3_x=ev3_y=ev3_z=0.0;

	Matrix U(3,3);
	DiagonalMatrix D(3);
	Matrix expM(3,3);
	U( 1, 1 ) = ev1_x;
	U( 2, 1 ) = ev1_y;
	U( 3, 1 ) = ev1_z;
	U( 1, 2 ) = ev2_x;
	U( 2, 2 ) = ev2_y;
	U( 3, 2 ) = ev2_z;
	U( 1, 3 ) = ev3_x;
	U( 2, 3 ) = ev3_y;
	U( 3, 3 ) = ev3_z;
	D(1) = exp( l1 );
	D(2) = exp( l2 );
	D(3) = exp( l3 );

	expM = U*D*U.t();

	t[0] = expM(1,1);
	t[1] = expM(1,2);
	t[2] = expM(1,3);
	t[3] = expM(2,2);
	t[4] = expM(2,3);
	t[5] = expM(3,3);

	t[6] = ( l1 - l2 ) / ( l1+ l2+ l3 );
	t[7] = ev1_x;
	t[8] = ev1_y;
	t[9] = ev1_z;
	//cout << "exp:  " << t[0] << " " << t[1] << " " << t[2] << " " << t[3] << " " << t[4] << " " << t[5] << " " << endl << endl;
	return t;
}

vector<float>Tracker::logT( size_t id )
{
	vector<float> t(6);
	Matrix U(3,3);
	DiagonalMatrix D(3);
	Matrix logM(3,3);

	U( 1, 1 ) = evec1[3*id];
	U( 2, 1 ) = evec1[3*id+1];
	U( 3, 1 ) = evec1[3*id+2];
	U( 1, 2 ) = evec2[3*id];
	U( 2, 2 ) = evec2[3*id+1];
	U( 3, 2 ) = evec2[3*id+2];
	U( 1, 3 ) = evec3[3*id];
	U( 2, 3 ) = evec3[3*id+1];
	U( 3, 3 ) = evec3[3*id+2];
	D(1) = log( eval1[id] );
	D(2) = log( eval2[id] );
	D(3) = log( eval3[id] );

	logM = U*D*U.t();

	t[0] = logM(1,1);
	t[1] = logM(1,2);
	t[2] = logM(1,3);
	t[3] = logM(2,2);
	t[4] = logM(2,3);
	t[5] = logM(3,3);
	//cout << "log:  " << t[0] << " " << t[1] << " " << t[2] << " " << t[3] << " " << t[4] << " " << t[5] << " " << endl << endl;
	return t;
}
